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PREFACE 


This report explores new uses of Kalman filter theory in manufac- 
turing systems (robotics in particular). 

The Kalman filter is a recursive algorithm to update estimates of 
the system's state variables using the current sensor measurements. The 
Kalman filter allows the robot to read its sensors plus external sensors 
and "learn''' from its experience. In effect, the robot is given a primi- 
tive intelligence. This work will focus on one application of Kalman 
filtering, namely, the calibration of a manipulator. This work is appli- 
cable to any type of powered kinematic linkage. 

The second section reviews the Kalman filter and its roodif ication 
for nonlinear systems. Section 3 gives a framework for generating the 
Kalman filter equations from the robot's kinematic equations. Once the 
equations are in the Kalman filter format, then the report demonstrates 
how the analysis of a calibration routine is done. The last section 
gives the derivation of the Kalman filter equations for a manipulator de- 
signed by the Computer Aided Design section of the Goddard Space Flight 
Center. Its unique kinematics caused special problems, and the solution 
to those problems is given. In the Appendix, the algebraic equations for 
evaluating the Kalman filter parameters are given. 
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SECTION 1 


INTRODUCTION 


This report attempts to link the seemingly unrelated fields of 
optimal recursive estimation {Kalman filters) and kinematics. The need 
for robotic manipulator control schemes linked kinematics and control 
theory, so, logically, estimation theory and kinematics should be com- 
bined to provide new insights into and capabilities for robots. Robot 
behavior must be viewed in a new light, separate from analyzing the 
high-bandwidth behavior that concerns control engineers. Actually gene- 
rating the robot's trajectory through space is not important; the end- 
poirVri of the trajectory are useful. This is where vital interactions 
b the robot and its environment usually occur. Consequently, the 
aetimation takes place in the discrete time domain. 

To implement a Kalman filter, an accurate state-space model of the 
system to be estimated must be formulated. This is the most challenging 
part of the estimation process and requires good engineering judgement. 
The system state vector, x, is the set of variables that completely de- 
scribes the system. If more variables are specified, they are redun- 
dant. If too few are specified, then some aspects of the system are 
ignored and this may affect both control and estimation tasks adversely. 
Coupled with a (nonunique) system model 


where 


x(k) 
f( ) 
u(k-1) 
w(k-1) 
k 


state vector at time step k 
a vector-valued function 
a control vector; the input vector at k 
a process noise vector; its presence makes jc(k) random 
a time index, "time step" 


the future state of the system may be predicted or extrapolated, ttie 
generic estimation task is to take the measurements Eq. (1-2) generates 


£(k> - h(k, x(k), u(k-1), v(k)) (1-2) 

where 

z(k) ■ a vector of measurements at time step k 

h_( ) ■ a vector valued function 

v^(k) - a process noise vector; the vector of sensor noise at 

time step k; its presence makes x(k) random 

and generate an updated estimate 


jc(k) ■ k{k, ac(k-l) , _u (k— 1) f z;(k) ) 


where 

A 

x(k) •* the estimated state vector at time step k 
k( ) - a vector valued function 


(1-3) 


For a robotic system, what set of variables completely describes 
the robot when it is at rest? One such set could be all the link 
lengths, twists, etc*, that describe the links in the kinematic chain. 


The actuator inputs must be added to this set. If the sensor does not 
measure any of these state variables directly, more parameters are needed 
to relate the robot to the sensors, and these parameters must help de- 
scribe the system. By collecting all these parameters together with all 
the functions that relate them to each other, the remote observer should 
know everything about the system at every point in time. However, not 
al?j these parameters are known exactly, and they must be either measured 
directly (along with some noise) or inferred from other noisy measure- 
ments, Thus, these parameters must be estimated . 

The engineer must derive a model, as in Eq. (1-1) and (1-2), that 
includes a specification of the state variables to be estimated. The 
state vector should not include perfectly known quantities or control 
variables like actuator inputs. Then, the estimator, Eq. (1-3) , takes 
over and generates updated estimates of the state vector. The rest of 
this report outlines the how's and why's of creating a system model, Eq. 
(1-1), and a measurement model, Eq. (1-2), for robots in general and the 
Goddard manipulator in particular. First, Section 2 reviews the mech- 
anics of the Kalman filter, a special form of Eq. (1-3). 
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SECTION 2 


KALMAN FILTER THEORY 


The Kalman filter is one of many estimation algorithms. It pro- 
vides the best linear fit of the state estimates to the sensor outputs 
for a linear system. A linear system mast be fit to the following 
form 

x(k) - *(k-1) xM) + B(k-1) u(k-1) + w(k-1 ) (2-V) 

zOO - H(k) xOO + v(k) (2-2) 

where 

$ ■ the state transition matrix. It may be time varing. If x 
is time invariant, $ is the identity matrix. It may not be 
a function of x or u 

B * the input weighting matrix. The zero matrix if the param- 
eters are time invariant 

H - the output weighting matrix. It cannot be a function x or u 

w - a zero-mean, Gaussian, white-noise process of covariance 
matrix Q (otherwise as defined in Section 1 ) 

\r » a zero-mean, Gaussian, white-noise process of covariance 
matrix R (otherwise as defined in Section 1 ) 

f t 
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and 


Q ■ E[ (ww T ) ] 

R » E[ (vv T ) I 

where 

Q ■ the process noise covariance matrix 
R * the sensor noise covariance matrix 
[E] * the expectation operator 

The Kalman filter is then defined as 


(2-3) 


(2-4) 


x(k) (+) - x(k)<-) + K(k) [z(k) - H(k)x (k) (-) J (2-5) 

where 

A 

jc(k) (+) * the estimate of x(k) after a measurement 

A 

ac(k) (-) » the estimate of x^(k) prior to a measurement 
K(k) * the Kalman gain matrix 

K(k) is derived by assigning a performance criterion, J, which is created 

■A 

by summing the square of the differences between jk^ and and minimizing 

A 

J with respect to K. The quantities x_(k) (-) and K(k) are computed as 
follows 


A A 


x(k) (-) - * (k-l)x(k-l) (+) + Bu(k-I) 


(2-6) 

M(k) » t(k-1)P(k-1)*(k-1) T + Q(k-1) 


(2-7) 

K(k) - M(k)H(k) T [H(k) M(k)H(k) T + R(k)]” 1 


(2-8) 

P(k) - [I - K(k)H(k) ]M(k) 


(2-9) 


5 ~ 


where 


POO - E[(x(k) - x(k)( + ))((x(lc) - x(k)( + )) T ] 

■ the estimation error covariance matrix after measurement 

M(k) - e[(xOO - x(k)(-))((x(k) - x(k)(-)) T ] 

■ the estimation error covariance matrix before measurement 

T - a superscript indicating matrix transpose 

These two variables chronicle the ignorance (or knowledge) of the filter 
about its state estimate over time. The greater P and M are, the less 
confident the filter is about its estimates. As this algorithm is re- 

A 

cursive, the starting values of £ and P must be given 

A 

Xq - E[x(0)] (2-10) 

P 0 - e[[x( 0) - ^)(x(0) - x 0 ) T ] ! (2-11) r 

Almost paradoxically, the engineer must provide the filter with his best 

A 

a priori knowledge (x/O)) about the system plus his ignorance about the 
system (Pq). Treating a robot as a stochastic, nonde ter mini Stic system 
is essentially a new concept for those who work with them. 

Kalman filtering has several distinct (but not necessarily exclu- 
sive) advantages. ? 

i 

(1 ) Every measurement vector need not be stored. It is operated § 

on once and discarded. Its information is used to update f 

•the state estimate vector. 

(2) Sensor output is properly weighted according to its noisi- f' 

ness. 1 

f; 

(3) The progress toward state estimate convergence to its true | 

value can be determined directly through the P matrix. f 

Estimation may cease when acceptably small covariance values 

are reached. | 

| 

i 

J- 
i 
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(4) Observability is also determined, and calibration routines 
may be checked to see i£ observability satisfied. Observa- 
bility is the property of the filter that determines wheth- 
er, in a fixed number of measurements, all the state esti- 
mates may be determined uniquely. Section 3 expands on 
this. 

In robot calibration, the states of the system that must be esti- 
mated are the robot's geometric parameters. These parameters are not ex- 
pected to vary over the duration of the calibration. Hence, 4 (k) is the 
identity matrix and Q and B are zero matrices. As will be shown, the 
greatest emphasis is on generating an output equation of the form of Eq. 
(1-2). To use a Kalman filter, Eq. (1-2) must be linearized to the form 
of Eq. (2-2) by utilizing 


H(k) - 


3h(k) 

3x00 


x(k) (-) 


( 2 - 12 ) 
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SECTION 3 


TRANSFORMING KINEMATIC EQUATIONS 
INTO KALMAN FILTER FORM 


3.1 Overview 

Kalman filter calibration of a given manipulator or robot consists 
of five steps: 

(1) Geometric modeling. 

(2) Selection of test sequence. 

(3) Design of fixtures and instrumentation. 

(4) Execution of test sequence and data collection. 

(5) Data reduction by Kalman filtering. 

The data reduction can be accomplished either on or off line as the con- 
trol vectors are generated in an open-loop fashion. If the updated esti- 
mates were required for generating control vectors, then on-line filter- 
ing would be necessary. The product of the filtering is updated esti- 
mates of geometric parameters such as link length, encoder /actuator 
biases, joint locations, etc. 

Selecting the parameters to be estimated requires an important en- 
gineering judgement. If too many parameters are selected, then computa- 
tion time increases and limits on available data memory are reached. The 
amount of storage needed increases as the square of the number of states 
(parameters) . On the other hand, selecting too few states may neglect 
important errors and the Kalman filter will attempt to project these er- 
rors onto the wrong parameters. The parameters should be selected on the 
basis of the likelihood of being in error (relative to current, uncali- 
brated estimates) and the importance of an error if it exists. 
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3. 2 Geometric Modeling 


vs 


The first task in calibrating the manipulator is to generate an 
accurate kinematic model. The desired form of the model will be the ma- 
trix equation 

ZTg? - BG (3-1 ) 

where 

Tg ■ the transformation that describes the state of the manipu- 
lator itself. For any manipulator, Tg is a function of 
the actuator inputs and the kinematics of the manipulator 

E ■ the transformation that describes the state of the end ef- 
fector. For example, on the Goddard manipulator, this would 
be the compliant instrument. The sensor outputs are a func- 
tion of this transformation 

B * the transformation that describes an object or feature rela- 
tive to the basic coordinate frame 

G - the transformation that describes the tool tip or end 
effector relative to B 

as suggested in Reference 2. 

For Kalman filtering, the highest interest is in generating equa- 
tions of the form 

£ - f_{ Tg, Z, B, G, E) + v (3-2) 

where £ and v are as described in Sections 1 and 2. For example, it may 
be derived from 

E(z) * T^Z^BG (3-3) 

— 6 

-1 -1 

which is Eq. (3-1) premultiplied by Z and T g , if the sensors were in 
the wrist. Equation (3-2) is derived by algebraic manipulation. The net 
result should be a set of equations of the form 
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where 


£ - a vector of parameters to be estimated 
** a vector of actuator inputs 

This is an explicit function of time, t, as the configuration may be 
changed in some way, in addition to changing actuator inputs. 

It may be desirable to calibrate the end effector/sensor and the 
manipulator separately, in this case, the instrumentation and equations 
will be quite different. It is, however, possible to calibrate both 
units simultaneously. 


3.3 Selection of Test Sequences 

The most important characteristic of the test sequence is that it 
satisfies the observability criterion. Thus, all errors in the parameter 
estimates are reflected in the sensor outputs and these errors are dis- 
tinguishable from one another. Mathematically, this can be determined as 
follows: create the observability matrix. A, where A is defined as 


HO 

Hi 


where 


H 


i 



i * 1,2,. ..,m 


If the rank of A * n, where n is the number of parameters to be esti- 
mated, then the system is observable with the given set of ro measure- 
ments. One cannot know, a priori, what the values of x are, so their 


initial estimates must be substituted when computing A. Should the ma- 
trix A have a rank less than n, then new measurements or configurations 
must be added until A has a rank of n. The nonlinear characteristics of 
h_, in general, will make this condition easy to satisfy as almost every 
parameter influences every sensor output to some degree. 

After observability has been satisfied, the test sequence can be 

repeated until sufficient convergence has been obtained. Some measures 

of convergence are the diagonal elements of the matrix, P, the estimation 

error covariance matrix. The smaller the value, the better known the 

parameter or state variable that corresponds to that diagonal element. A 

2 

rough rule of thumb to translate the variance, a^, into a ± range (from a 
normal to uniform probability distribution with identical variances) is 

r L « (3/4 ) 1/2 ^ (3-6) 

where the estimate is in error by ±r. If the variance is not getting 
smaller, then the observability criterion is not being met or the system 
is so noisy that the initial estimate is better than subsequent ones. 

The latter case is relevant only if the parameters change with time. 

Some parameters converge very slowly because their errors make 
small projections into the sensor output. This can be seen in the column 
of the H matrix that corresponds to that state variable. If the value is 
small relative to that variable, then convergence will be slow. The only 
way to counter this is to find inputs and configurations that will boost 
the values in that column. 

3.4 Design of Instrumentation and Fixtures 

The key words for successful calibration are control, simplicity, 
and accuracy. Any errors not associated with the parameters to be esti- 
mated will be attributed erroneously to the estimated parameters. Hence, 
the estimates will be wrong. So, Control of the calibration is impor- 
tant. Fixtures must be tight and rigid, instruments must be as accurate 
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and bias free as possible. Simplicity is important because complex cali- 
bration apparatus that also needs to be calibrated is unwanted. It is 
better to take more measurements from fewer sensors than to try to con- 
trol and read many sensors a fewer number of times. Use of gauge blocks 
and other fixed references is desirable if there is a compliance to ab- 
sorb the position errors. 

Whether the instruments are man read or machine read is not impor- 
tant as long as they are accurate, properly placed, and properly mod- 
eled. Naturally, computers often can read sensors much faster than hu- 
mans. 

3 . 5 Data Acguistion 

Once the sensors are in place, the computer must be ready to ac- 
cept the data. For off-line computation, the sensor readings and the 
commands to the actuators must be recorded. For on-line computation, the 
instrument readings and the actuators' control vector may be used direct- 
ly in the Kalman filter. When computation is complete for one point, the 
next actuator control vector may be computed and sent to the manipulator. 

3.6 Conclusion 

Kalman filtering offers the following advantages: 

(1) It does not require storage of individual data points. 

(2) It allows prior knowledge to be included. 

(3) It indicates convergence. 

(4) It allows for noisy seniors and multiple sensors. 

i 

(5) It verifies selection of a fully observable ensemble of 
calibration points. 

As with any calibration, knowledge of the calibrated device, careful con- 
trol in data collection and fixturing, and accuracy of instrumentation is 
important for success. 
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3.6 


Review 


This section has discussed in general terms how the kinematic 
equations should be manipulated for the Kalman filter. In practice, 
finding Eq. (3-4) may be difficult, if not impossible. At best, the 
equations are very complex and nonlinear. This is the price to be paid 
for a detailed calibration. The next section derives the detailed equa- 
tions for calibrating the Goddard manipulator. 
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SECTION 4 


ANALYSIS OP THE GODDARD MANIPULATOR 


Generating the Algebraic Equations 

The fact that the Goddard manipulator, Figure 4-1, is a closed 
kinematic linkage makes its analysis quite difficult. An analysis done 
using Eq. (3-1) is more applicable to open kinematic linkages that char- 
acterize most industrial robots. 4x4 transformations will be used in a 


tSZgl 


Figure 4-1. The Goddard manipulator 



slightly modified form. This section introduces a substantial amount of 
new nomenclature. Reference 2 defines the terms Tran3 [ ] , Rot £ ] , and 
Euler [ ]; the other nomenclature used herein maintains the standard that 
t(4) is the fourth element of vector 1:, etc. 

First/ it is necessary to find all of the many unique transforma- 
tions that characterize the manipulator kinematically. Little precision 
in manufacture is presumed; this necessitates the calibration. For ex- 
ample/ each mounting pad of each link has a fixed offset transformation 
relative to one of two hypothetical origins (either for the fixed base or 
the moving platform). Another transformation relates the mounting-^pad 
origin to the origin of the coordinate frame in which each joint at the 
link ends is found (see Figure 4-2). Each of these offset transforma- 
tions is a function of no more than six variables, and in total, these 33 
variables provide most of the state vector shown in Eq. (1-1). This 
effort results in a general relationship for finding the transformations 
that describe the telescoping links (see Figure 4-3). 

Second, one finds the functions that relate the link transforma- 
tions to the actuator positions, u. This involves seven more state vari- 
ables than those found in the first set of 33 equations. 

The third part of the analysis derives three equations that relate 
the sensor outputs to the link transformations. This adds three more 
state variables which yields a total of 43 possible state variables. The 
sensor equations presume that potentiometers are mounted at the bottom 
pivots of three links. 

Fourth, one combines the equations and derives a set of three 
equations for the sensor outputs, z_, as a function of the state vari- 
ables, _x, and the actuator inputs, ju. There is no obvious method for 
producing this combination, and, fortunately, one need not exist. It is 
necessary only to compute jz and the derivative of the output function 
with respect to the state vector. z_ c v « be computed using the previous 
mass of equations, and the derivative of x can be found using the chain 
rule. This process, however, yields a 45 x 45-element matrix which, 
apparently, must be inverted. 
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Figure 4-3. Transformation graph. 

Fifth, however, this formidable problem can, in this case, be re- 1 

' a 

.ft 

duced to one of inverting a 6 x 6 matrix. The form of the transfor- 5 

mation equations and selecting proper intermediate variables makes the 
reduction possible. 

Sixth, an easier method of computing the necessary differentials 
is described. The Appendix contains the set of analytic equations used 
to compute the differentials. The need for a simpler computation scheme 
is self evident. 


ORIGINAL PAGE B 

OF POOR QUALITY 


The first task is to establish a set of coordinate frames. Refer- 
ring to Figure 4-1, the origin of the manipulator base will be defined as 
a point in thtf plane defined by the three points midway between the cen- 
ter of rotation of the pivots at the base of adjacent links. This mid- 
point for base LI is on the y axis and on the x axis for the L2 and L3 
bases. The top of the ‘manipulator has a similarly defined origin, base 
U2 is on the x axis and 01 and U3 are on the y axis of the upper coordi- 
nate frame. The 4x4 transformation that relates the top platform to 
the bottom platform is T such that 


where 

Trans [ ] 
Euler [ ] 


T - Trans [t(1 ), t(2), t<3)] 

• Euler { t (4 ) , t(5), t(6> 3 


the translational transformation 
the Euler angle transformation 


In matrix form 


Trans [(t(1 ), t<2), t(3)I 


1 0 0 t(1) 

0 1 0 t(2) 

0 0 1 t(3) 

0 0 0 1 


(4-1 ) 


(4-2) 


and 

Euler [t(4), t(5), t(6)] « Rot [z,t(4)I Rot [y,t(5)l Rot tz,t(6)] 

(4-3) 
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The Rot tranformations are 


Rot [z,t(4)3 


r 

icos t(4 ) 


-sin t(4 ) 0 0 


sin t(4) cos t(4) 0 0 


1 0 


0 1 


(4-4) 


Rot [y, t(5 ) 3 


cos t(5) 0 sin t(5) 0 


-sin t(5) 0 cos t(t) 0 


0 0 


(4-5) 


Rot [z,t(6)3 


cos t(6) -sin t(6) 0 0 

sin t(6) cos t(6) 0 0 


1 0 


0 1 


(4-6) 


As a result, the vector t, from Eq. (4-1) or the transformation T can 
describe the origin of the top platform relative to the origin of the 
bottom platform completely. Also, using the transformation in Eq. (4-7), 
the vector £ in the top platform can be known as the vector r_ in the bot- 
tom platform. 
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r =• 


T • 


(4-7) 


The Euler rotation transformation is one of a number of possible rotation 

transformations • It is used herein because it is so common* 

•• 

Similarly, a platform's origin can be defined relative to each of 
its three bases with a transformation 

TOP - Trans [tp(1), tp(2), 0] 

• Euler [ tp ( 4 ) , tp ( 5 ) , tp(6)) (4-8) 

for the basis U1 , U2, U3 relative to top platform and 

BOT - Euler [tb(3), tb(4), tb(5)] 

• Trans [ tb ( 1 ) , tb(2), 0] (4-9) 

for the origin of the bottom platform relative to the bases LI, L2, L3. 
Putting the transformations in this order make tp(1), tp(2), tb(1), and 
tb(2) into x and y coordinates in their respective platform planes. 

For any base, either tp(1 ) or tp(2) [tb(1) or tb(2)] may be 
selected uniquely. For each base, its vector tg (or tb ) will uniquely 
define its transformation relative to the origin of its respective plat- 
form. For a given base,, the z axis is normal to the surface of the base, 
the y axis points to the center of the platform, and the x axis inter- 
sects adjacent pivots. Note that the base's origin lies above its sur- 
face. 

The lower bases are, in turn, related to the bottom pivot points 
of the links by a simple rotation about the z axis and a translation 

JL - Trans Ej£(1), 0, 0] • Rot tz, j£(2)l (4-10) 

The upper pivot points of the links are related to their bases by a 
translation 

JU * Trans (ju, 0, 0) (4-11) 
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Each pivot bearing lies in a different coordinate frame than the base to 
which it is attached. The z axes are parallel and the y axes still point 
inward, but the x axes of the bottom bases are the axes of rotation of 
the lowest bearings of the spiders. These are the axes for the angle a 
of Figure 4-2. The y axes of the top pivot bearings are the center of 
rotation of the topmost bearing, about which the angle C is measured. 

Here, the coordinate frame is just shifted along the x axis. 

The variables jJl(1), j A ( 2 ) , and ju do not vary from base to base 
if a template is used to locate the position of the pivots relative to 
each other on the base. For all of the other vectors described, such 
uniformity may not be guaranteed, hence there are three TOPS and three 
BOTs, one for each base. Correspondingly, there are three tb vectors and 
three J^> vectors. 

Enough transformations have been defined to derive the transforma- 
tion that represents the transformation for a single link . 

JT - JL • BOT • T • TOP • JU (4-12) 

The truth of this equation becomes evident if one traces the two equiva- 
lent transformation paths from the bottom bearing coordinate frame to the 
top bearing coordinate frame. One path is direct and is called JT. The 
other path goes from the bottom bearing to its base (JL), from this base 
to the bottom platform origin (BOT), from the bottom platform origin to 
the top platform origin (T), from the top platform origin to the top base 
(TOP), and, finally, from the top base to the top bearing coordinate 
frame (JU). These two paths are equivalent, hence the product of the set 
of relative transformations is equivalent to the direct transformation as 
in Eq. (4-12). This equation is the objective of the first step in the 
analysis. 
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This identity allows us to determine the extension and rotation of 
a link based on T and four other transformations* The transformation JT 
can be expressed explicitly as 


11 

*12 

*13 

P 1 

21 

r 22 

r 23 

P 2 

31 

r 32 

r 33 

P 3 

0 

0 

0 

1 


(4-13) 


The variables r^j and p^ are functions of JU, TOP, T, BOT, and JL. 

In addition to Eq. (4-12), JT also is a product of a third set of trans- 
formations that identify each translation and rotation of the link's 
parts T 

JT ■ Rot [x,a] • Rot ty,&] • Rot [z, 8] 

• Trans [z,L + s(9+u)] • Rot [x,ril 

• Rot [y,5l (4-14) 

The variable u is the actuator angle, s is the pitch of the lead screw 
and L is the link's unextended length* 

The origin of this equation is an analysis similar to the one that 
generated Eq. (4-12). Starting at the coordinate frame of the bottom 
bearing, rotate the assembly a radians about the bearing's x axis. The z 
and y axes are then rotated in a vertical plane. Relative to the new 
coordinate frame, the assembly is rotated 8 radians about the new y 
axis. This is the other bearing of the spider. Consequently, the z and 
x axes are rotated in a plane that intersects the base at an angle of a 
radians. The z axis now points to the top pivots 
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Now, rotate the remaining portion of the assembly 0 radians about 
the z axis. This is the rotation of the lead screw's nut relative to the 
actuator. This is not the same as the actuator angle, u. The next step 
is a translation that pushes the top base L + s(0 + u) units along the 
current z axis. Two more rotations reorient the top bearing (a rotation 
of n radians about the newest x axis and a rotation of ? radians about 
the y axis of the top bearing). As for Eq. (4-12), this exercise yields 
a product of relative transformatins that is equivalent to the direct 
transformation, JT. 

The angles a, 8, n, C, and 0 are not really important; the quanti- 
ty that must be known is u as a function of JT, or more explicitly, u as 

a function p^, p 2 , p 3 , r 12 , r 22 , and r 32 « Therefore, Eq. (4-14) should 
be solved for all actuator angles, u. Multiplying through the right, side 

of Eq. (4-14) and equating with the consonants of JT, one finds six equa- 

tions for the six dependent variables a, 0, £, n, 6, and u. The three 
designating the translational part are 


Solving nin 


PI 

■ sin 0 (s(u + 0 ) + l) 

(4-15) 

P2 

■ sin a cos 0 (s(u +0) + l) 

(4-16) 

P3 

■ cos a cos 0 (s(u +0) + l) 

(4-17) 

sin 

0, and the cosines 



P 3 

cos a * 

(4-1 8> 


, 2 . 2 
/p 2 + p 3 


cos 0 




2 

3 


+ P 


2 

3 


(4-19) 


23 


sin a 


(4-20) 


plus 





sin 0 





(4-21 ) 


+ i>2 + P 3 “ s(u + 0) + L 


(4-22) 


If Eq. (4-18) through (4-21) are substituted into the transforms of Bq. 
(4-14), then the identity can be rewritten as 

Rot 1 [y,01 • Rot " 1 tx,o) • JT » Rot [z, 8 ] 

• Trans [z,L + s(0 + u)3 

• Rot [x,r|) • Rot [y,?] (4-23) 


and the left side is completely determined. Multiplying the right side 
of Bq. (4-23) through again and equating the elements of Rows 1 and 2, 
Column 2 

cos & r 12 - sin 0 (cos a r 32 - sin a r 22 ) =» - cos h sin 0 (4-24) 


r„„ sin a + cos a - cos r\ cos 0 
32 22 


(4-25) 


Dividing Eq. (4-24) by (4-25) 


tan 0 


sin 0 r„_ cos a - r„ sin o - r,_ cos 0 
32 22 12 

r„ sin 0 + r__ cos a 
32 22 


(4-26) 
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Substituting Eg. (4-18) through (4-21) into Eq. (4-26) and inverting the 
function 


0 • tan 


-1 


p 1 p 3 r 32 + p,P 2 r 22 - (p 2 + P 3 ) r 12 


t p 3 r 22 - P 2 r 32 ) / P, + P 2 * P 3 


(4-27) 


Finally, Eg. (4-22) can be combined with Eg. (4-27) to 1 derive 


p l p 3 r 32 * p l p 2 r 22 ~ C*2 + P 3^ r 12 

_ ( p 3 r 22 ' P 2 r 3 2 ) /p 2 + p 2 + p 3 _ 

(4-28) 

This allows the actuator angle, u, to be determined for any r, , , p. which 

x 

are based on T. 


u 


,2 2 2 

V Pl + P 2 + P 3 " L 


- tan 


-1 


To review, there are six sets of six independent equations each 
that describe the transformations of the individual links as a function 
of T and a number of fixed transformations that come from applying Eq. 
(4-12) to each link. It is possible to define a vector which comr 
prises a set of six independent parameters of the transformation JT such 
that, for each link i 


12 
*22 


(4-29) 
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Consequently, jj[ is a function of T and the link's BOT, TOP, JL, and JU 
transformation are 

TOP i , BOT if JL i , JU^ (4-30) 

where 

i m 1,2, 3,..., 6 

The subscript i is used to designate that there are six such vector val- 
ued functions, one for each link. The TOP, BOT, JL, and JU transforma- 
tions are combined uniquely for each link. 

Equation (4-28) gives the second set of equations that relates the 
transformation of an individual link to its actuator angle 

u i - L i» s i) (4-31) 

These are six scalar equations. If these equations were combined with 
Eq. (4-30) and the jj/s eliminated from them, then the u^'s would be 
a function of T alone (assuming TOP, BOT, JL, and JU are known constant 
transformations). Conceivably, one could attempt to solve for T as a 
function of u., but this may be impossible, and it is not necessary for 
tile estimation task. The calibration needs the differential of the out- 
put vector, z, with respect to the state vector, _x, to find the H matrix. 

Thus far, what the sensors should be or where they should be 
placed has not been mentioned. The sensors should measure state vari- 
ables directly if possible, without adding extra functions and, there- 
fore, potential state variables. In this case, the state variables 
cannot be measured directly. The easiest variables to measure are the 
angles a and 8 at the base of each link. Subsequent analysis- supposes 
that three of the a angles (of links 1, 2, and 3/ were instrumented with 
rotary encoders or potentiometers. 

sin (z . + a , ) 

3 j 


Then, from Eq. (4-20) 


-Pr 


, 2+2 
/p 2 P 3 


(4-32) 
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where 


z . ■ the measurement 

D 

a^ ■ a bias 
j - an index of sensors 

The lower case a's are the remaining three state variables, and this is 
the only set of equations needed to complete the third step of the analy- 
sis. 


4u2 Solution of the Implicit Functions 

The next question is, what rext? The output Eq. (4-32) has been 
found, but this expression can not be differentiated with respect to 
state variables that do not appear in the equation. The problem is that 
Eq. (4-32), (4-31), and (4-30) comprise a set of implicit functions — a 
total of 45 scalar equations in 49 independent variables (the elements of 
the vector u, and all the state variables) and 45 dependent variables 
(the elements of the vectors j, , j_ 2 , j_ 3 » j^, t, and z_). The full 

set of state variables is 


T 

x 


t) 


T T T T T T 
ifiy J-E-2' -^3' — 1 ' — 2 ' —3' a i ' a 2' a 3' 


(4-33) 

There is a mathematical relation called the chain rule that allows 
one to find the partial derivative of a dependent variable with respect 
to an independent variable without solving the functions involved. This 
is important because the derivative of z_ (a dependent variable ) with 
respect to the state variables (independent variables) is needed to 
create the H matrix used in the Kalman filter. The following is the 


s, ju 1 , ju 2 , ju 3 , ji^, j £ 3 


fourth part of this analysis. First, three sets of implicit functions 
are created from Eq. (4-12), (4-28), and (4-32). 


o - till' *ki' Jv iii) - li 

0 - L i; .J - Ul 


where 


and 


1, 2, r • • ,6 


0 - hj - sin (zj + aj) + 


/ 2 2 
/P 2 * P 3 


—f 3 


where 


j - 1, 2, 3 

The chain rule allows us to create 


3 ^1 


(-) 


3x 


3f. 


3x 

r 

• • * 

3a 

3x 

r 

•- • • 

3h 

3x 


where 



*&- 

3i i 

• 

a 

3 -1 

at 

• 

az 


i. 

0 

. . 

i 

*Je 

IM 

3 A, 

3 i* 

at 

3z 


3a . 

. . 3 i ' 

3 1 



3 A, 

3 i6 

at 

3jZ 


3h . 

. . % 

ah 

3h 


_ 3 A, 


3t 

a7 

the 

n^ element of the 

state 

vector 



3 ii 


ax 

n 

a 


i 

• 

ax 

n 


3t“ 


ax 

n 


dz_ 


3 >r 

n 


(4-34) 

(4-35) 


(4-36) 


(4-37) 
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See Reference 3 for details. Note that the la&'fe element of the vector on 
the right side of Eq. (4-37) is the variable sought. The large matrix 
may be computed numerically as may the vector on the left. This computa- 
tion requires a 45 x 45 matrix to be inverted to find 3z/3x n which is 
not practical. Fortunately, this matrix may be simplified so that the 
largest matrix inversion is on the order of 6 x 6. The following para- 
graphs explain how this is done. 

Refer to Figure 4-4. It is equivalent to Eq. (4-37) after the 
differentials have been evaluated and assigned to the given submatrices. 
Note that most of the matrix is empty. Some useful simplifications may 
then be made. This is the fifth step in the analysis. First, presume 
that x n is not found in or h, which is the case for 33 of the vari- 
ables. Each of the differentials 

3 i± 

n 


may be found by rearranging the first six rows of submatrices to yield 


3 ii 

G i 3x " ^in 3x“ 
n n 

where 

* a 6 x 6 matrix 
i * 1, 2, ... ,6 

F. » (-)3f,/3x » a 6-element vector 

—in -4/ n 


(4-38) 









Distributing D and factoring out 3t/3x n 


D, F, 

1 —In 


D C 
1 1 

• 

m 

• 

• 


• 

D F 

6 i-6n 


°6 C 6 

_ 





3t_ 

3x 


(4-40) 


Then, the matrix on the right side may be inverted. It is only a 6 x 6 
matrix. Inverting this matrix and premultiplying each side with it re- 
sults in a numerical value for 3t/3x n . If 3j^/3x n is known, then the 
left side of Eq. (4-38) is determined completely so it may be inserted 
into the h_ equations 

3t 


Y i (c i ?r - 

n 


- B 


1 3x 


(4-41 ) 


Y 2 <c 2 17 - W 

n 


- B 


3 3x 


(4-42) 


where 


Y 3 <°3 IT" ' F 3„> * - B 

n 


3kj/3j_j ■ a 1 x 6 matrix 
3kj/3z_. - a scalar quantity 


3*3 

3 3x 


(4-43) 


Consequently 


3x 


3*3 

3x 


3t “J 

C 1 3^ n " F 1n| 
C 2 Sx^ ‘ F 2n| 


(4-44) 


(4-45) 


~ 3t “ 

C - P 

3 3x -3n 
n 


(4-45) 


The left side of these three equations gives the elements of H that are 
necessary for this analysis. The matrix 


D C 
1 1 


D C 
6 6 


is evaluated once each time step, the value of 3t/3x n must be evaluated 
for each state variable, as is Fj. n , and both must be evaluated every 
time step. Thus 


3z(k) 

3x00 


IxOO(-) 


which is the desired form for the Kalman filter. This allows- estimation 
of the values of the 43 state variables. If it is judged that fewer 
states are required, then these states may be omitted without Eq. (4-44) 
through (4-47) losing their generality. 
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There are a few state variables that only appear in functions % 
and h. By a similar analysis as above, this relation results for state 
variables that appear in c[ 


G 


D C 
1 1 


D C 
6 6 


9_t 

sir 


n 


(4-49) 


where 

» (-) Sg/Sx^ »• a 6-element vector 


Consequently 


and 


3t 

sir 


i-i 


D C 
1 1 


D C 
6 6 


3z. 


3t 


3x 


n 


B. 


'1 3x 


n 


3z. 


3t 


3x 


'2 3x 


3z. 


3t 


3x 


n 


B. 


'3 3x 


(4-50) 


(4-51 ) 


(4-52) 


(4-53) 
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If the state variable appears in h only, like the aj's, then 


D C 
1 1 


°6 C 6 


at 

Sx 


n 


( 4 - 54 ) 


Since the matrix on the right side is nonsingular 



so 



G 

B 


1 

1 



( 4 - 55 ) 


( 4 - 56 ) 


( 4 - 57 ) 


( 4 - 58 ) 


It should be noted that additional sensors may be added with 
little increase in the complexity of this system. Putting sensors on all 
six joints would speed up the convergence of the state estimates. An- 
other interesting feature is that the values of 

at 

3u 

are generated if the _u's are treated like x n 's. This is the manipula- 
tor's Jacobian matrix, which relates the differential change in its Car- 
tesian coordinates with changes in its joint angles. 
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The remaining challenge is to evaluate the differentials of the 

functions jj, and h. The MACSYMA symbolic algebraic manipulation 

program was us-ad for this evaluation. The functions for evaluating the 

elements of the matrices F. , E , G , C . , D . , Y . , B . are given in the 

—in n n i' i i i * 

Appendix. For the differentials of the F^ n and matrices, the 
functions are quite lenqthy. Clearly, these equations should not be 
programmed into a computer in order to compute the lin’s and C^'s. 

The following is the sixth and final part of the analysis. 

A simpler method of deriving the differentials comes from the 
Teleoperator Arm Design Theory (TOAD). (4) to set up the equations for 
this, one must remember that determining the differential of a trans- 
formation with respect to a variable is equivalent to finding the differ- 
ential with respect to each of its matrix elements. So, if the differ- 
ential of the elements of JT with respect to a state variable or depend- 
ent variable is found, then the differential with respect to JT is found 
and vice versa. Recall Eq. (4-12). The differential of the right side 
with respect to the state variables would also give the differential of 
JT with respect to the state variables. If the right side is factored 
into all its single-axis rotations and single-direction translations, as 
for Eq. (4-1), then each state or dependent variable will appear in only 
one of these factors. So, if the differential of the matrix with respect 
to its variable is put in its place, then the new product is the differ- 
ential of the old product with respect to the state variable. 

The aforementioned differential matrix needs not be evaluated; a 
single matrix operator may be used to premultiply the transformation that 
will result in Che differential, i.e. 




3x 


m 


N . A . 
i i 


where 

1*^ » the matrix operator 


(4-59) 
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If Aj_ is an x translation, then 


N 


0 0 0 1 
0 0 0 0 
0 0 0 0 
0 0 0 0 


( 4 - 60 ) 


If Ai is a y translation, then 


N 


0 0 0 0 
0 0 0 1 
0 0 0 0 
0 0 0 0 


( 4-61 ) 


If Aj, is a z translation, then 


N 


0 0 0 0 
0 0 0 0 
0 0 0 1 
0 0 0 0 


( 4 - 62 ) 


If Aj_ is a rotation about the x axis, then 


N 


0 0 0 0 

0 0-10 
0 0 0 0 

0 0 0 0 


( 4 - 63 ) 
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If is a rotation about the y axis, then 

0 0 10 

0 0 0 0 

N « 

-10 0 0 
0 0 0 0 


(4-64) 


And, if A^ is a rotation about the z axis, then 


N 


0-100 
10 0 0 
0 0 0 0 

0 0 0 0 


(4-65) 


The benefits of this technique are obvious; the equations are more com- 
pact, and they result from transformations that Would have to be computed 
for any case. 

The numerical confutation of the submatrices Fj_ n and Cj_ should 
be performed using TOAD. Factoring Eg. (4-12) into its individual single 
variable transformations will achieve this, and it is possible for all 
homogeneous transformations. Equation (4-12) starts with the five basic 
subtransf ormtions, each of which can be decomposed into up to six single 
variable transformations as in Eq. (4-1). Thus the derivative of any JT 
with respect to a single variable is the product of the 17 elemental 
transformations, with the approximate N-operator premultiplying the ele- 
mental transformation that contains the variable of interest. The values 
of the derivative of the JE^'s with respect to the state variables and t 
are selected from the derivative of JT^ with respect to the state vari- 
ables and t . If the variable is a state variable, it goes in F^. If 
it is an element of the _t vector, it goes in a C^. 
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This derivative trust be computed for each state variable and t_ 
element of the vector _2i for each joint transformation JT^. This is 
a substantial amount of computation, but the alternative is to implement 
the analytic equations found in the Appendix. 

The TOAD technique works for homogeneous transformations only. 

The submatrices D, B, Y, and the vectors G, and E_ must be computed using 
the analytic equations found in the Appendix. 

The following is a possible sequence of the computations needed 
for calibration. 

(1) Select a T transformation to which the manipulator will be 

moved. Using the current estimates of transformations JL, 
JU, TOP, and BOT compute all and then Move the 

manipulator to the joint angles u^. 

(2) Compute the numerical values of all D^, Yj, and Bj 
using the analytic equations, again use the current 
estimates of the state variables and joint variables. 

(3) Use TCAD to compute the values of the submatrices C^. 

This involves setting up the decomposed product of the ele- 
mental transformations for each joint. Then, insert the 
matrix operators into the product for each of the t. ele- 
ments, and the new product is evaluated. The derivatives 
are selected from the matrix-valued differentials yielding 
one column of a matrix for each element/JT^ combin- 
ation. 

(4) Evaluate the matrix inverse of Eq. (4-47). 

Repeat the following five steps for each state variable. 

(5) Compute the values of the vectors and E^ using the analytic 
equations in the Appendix. If the state variable is not 
found in the equations, its derivative is zero. There are 
new G, J!, and Fj[ vectors for each state variable. 
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(6) Compute the values of the vectors using TOAD. These 
are best computed and stored when Step 3 is executed and 
should be retrieved now. 

(7) Compute the vector 

3t 

air 

n 

using Eq. (4-40), (4-50), or (4-55). 

(8) Compute the values of 

3jz 

alT 

n 

using Eq. (4-44), (4-51), or (4-57). 

(9) Insert these values into the H(k) matrix. 

When the H(k) matrix is complete, follow the next four steps. 

(10) Compute the Kalman-filter gain using Eq. (2-7) to (2-9). 

(11) Compute the expected value of the sensor outputs based on 
the value of T (or equivalently, _u). 

(12) Read the sensors, and update the state estimates using Eq. 
(2-13). 

(13) Return to Step 1. 

This paradigm may be optimized to ease computation of course. At 
Step 1, the value of T was assumed to be given. Its selection for each 
time step is based on the ability to satisfy the observability crite- 
rion. One can estimate whether any sequence of T transformations can 
satisfy observability a priori, but selecting a set of T's to satisfy 
observability is much trickier. The only sensible way to pick T is at 
random and then monitor the resulting H(k) matrices for observability 
after the fact. 
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SECTION 5 


CONCLUSIONS AND RECOMMENDATIONS 


The sum of the work presented is the equations contained in Sec- 
tion 4 and the Appendix. The manner in which these equations are derived 
should have an analog in the analysis of all closed-link kinematic link- 
ages. The particular equations are peculiar to this manipulator config- 
uration. The manipulator may be scaled up or down in size, and the gen- 
erality is not lost. 

The quantity of computation needed is substantial. This will 
obviously slow the speed at which a calibration can be done. A more 
insidious problem will be how to guarantee the stability of the filter, 
expecially one with up to 43 state variables. If the initial estimates 
of state variables are far from their true values, the the filter could 
converge to unreasonable state estimates or drift about state space with- 
out converging. It is important to provide as accurate an initial esti- 
mate as possible to help justify the linearization of the equations. 

How useful the Kalman filter is for solving problems of this sort 
and magnitude has not been simulated or experimentally verified. Should 
the filter fail to give reasonable results, two techniques for improving 
the estimation are offered: 

(1 ) Degenerate the model and limit the number of state vari- 
ables. Pick a set of critical state variables and presume 
the others are fixed and accurate. The estimated state 
variables in the new model will reflect any errors in the 
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unestimated state variables in the full model. In the 
paradigm described at the end of Section 4, steps 5 through 
9 will be executed a fewer number of times. The encoder 
biases, a-j, and the unextertded joint lengths, L^, are 
likely candidates for this type Of state-variable subset. 
They are, respectively, offsets in sensors and inputs 
(actuators). These yield quick fixes for whatever working 
kinematic model is used in practice. 

( 2) Compute the H(k) matrix based on the initial estimates of 

the state variables. Should the state estimate diverge, the 
Kalman gains will still be based on reasonable values of the 
state. 

All ejq<erimental data should be saved, so the examples can be rerun with 
a simpler model or different initial conditions. 

The numerical values of Fj. and should be confuted using 

TOAD. The numerical values of Gy IS, D^, Yj, and Bj should be found 
by using analytic equations found in the Appendix. This avoids program- 
ming a very large set of equations into a computer. Hie programmer must 
find the best way to implement the equations given. 


APPENDIX 


This appendix contains formulas for the differentials of 

3f 3f 


Eqs. (4-33), (4-34), (4-35). 
3f 


The differentials for 




33 im’ 


and 




( 2 ) 


were not computed. 


Key 


la = 

3 * " 


partial derivative of the expression g, with respect to * 


■jj-y s partial derivative of the expression h, with respect to * 


3f (k) 
3 * 


partial derivative of the kth element of the vector-valued 
function, f, with respect to *. 
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g- equations (Eq. 4.30) 
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COS <2 


- equations (Eq. 4.29) 



(cos (tb (3) ) cos(tb(4)) cos (tb(5) ) - sin(tb(3)) sin(tb(5))) 
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